#include <Arduino.h>
#include <ESP32Servo.h>
#include <Ticker.h>
Ticker timer_read_encoder;  // 中断函数定时器定义,声明ticker对象

#define ENCODER_L   12 //编码器
#define DIRECTION_L 13
#define interrupt_time 100 // 中断时间
#define AIN1_PIN 26   //电机
#define AIN2_PIN 25
#define potPin   27  //ADC
#define duoji_PIN 14
#define led 2

Servo servo1;     
static  int  servo=0;
String mycommand= "" ;
int potValue;
int Velocity_L, Velocity_Left, target_val, output_val; //速度
float Kp, Ki, Kd, err, err_last, integral; 

void READ_ENCODER_L(void);
void sent(void);
int pid_realize(int actual_val);

void setup() {
  Serial.begin(9600);
  pinMode(ENCODER_L, INPUT_PULLUP);       //编码器引脚 输入模式
  pinMode(ENCODER_L, INPUT);       //编码器引脚 输入模式
  pinMode(DIRECTION_L, INPUT);     //编码器引脚 输入模式
  pinMode(AIN1_PIN, OUTPUT);
  pinMode(AIN2_PIN, OUTPUT);
  pinMode(led, OUTPUT);
  digitalWrite(AIN2_PIN, LOW);
  servo1.attach(duoji_PIN, 1000, 2000);
  attachInterrupt(ENCODER_L, READ_ENCODER_L, CHANGE);  //中断函数READ_ENCODER_L
  interrupts();                      //打开外部中断  
  timer_read_encoder.attach_ms(interrupt_time, sent);  
}

void loop() {
  sent();
  pid_realize(Velocity_Left);
  analogWrite(AIN1_PIN,output_val);
//  potValue = analogRead(potPin);
//  Serial.print(String(potValue) + "c");
  while (Serial.available()>0)
     {
       mycommand += char (Serial.read());
       delay(2);
//       char inChar = (char)Serial.read();
//      if ((inChar)== 0x81) {digitalWrite(led,HIGH);Serial.println("led on");}
//      else if ((inChar)==0x01) {digitalWrite(led,LOW);Serial.println("led off");}
     }

  for ( int  m=0;m<mycommand.length();m++)  // 
    {
       char  ch = mycommand[m];    //读取串口数据
       switch (ch)
          {
              case  '0' ... '9' :
                    servo = servo * 10 + ch -  '0' ;    //字符转换成十进制
                   break ;
              case  'a' :    //如果数据后带a，则表示是一号舵机的数据，比如串口发送85a
                  if(servo >= 0 && servo <= 180 )   
                     {servo1.write(servo);}                
                  servo = 0;
                  break ;
              case 'b' :
                  target_val = servo;
                  break ;
              case 'p' :
                  Kp = servo / 1000;
                  servo = 0;
                  break ;
              case 'i' :
                  Ki = servo / 1000;
                  servo = 0;
                  break ;
              case 'd' :
                  Kd = servo / 1000;
                  servo = 0;
                  break ;
              default:
                  servo = 0;
                  break;
          }
   }  
    mycommand= "" ;
    delay(100);
}

/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void READ_ENCODER_L(void) {
  if (digitalRead(ENCODER_L) == LOW) {     //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L--;  //根据另外一相电平判定方向
    else      Velocity_L++;
  }
  else {     //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L++; //根据另外一相电平判定方向
    else     Velocity_L--; 
  }
}
/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void sent(void)
{
  Velocity_Left = Velocity_L*100/interrupt_time;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
  Serial.print(String(Velocity_Left) + "s");
//  Serial.println(String(Velocity_Left)+"编码器");
}

int pid_realize(int actual_val)
{
  err = target_val - actual_val;
  /*积分项*/
  integral += err;
  /*PID算法实现*/
  output_val = Kp * err + 
             Ki * integral + 
             Kd * (err - err_last);
  /*误差传递*/
  err_last = err;
  /*返回当前实际值*/
  if (output_val >255)
  output_val = 255 ;
  if (output_val < 0)
  output_val = 0;
  Serial.println("output_val"+String(output_val));
  return output_val;
  }
